Coursera自动驾驶2.3

旋转变换可以有很多方式来表示。 最常见的是使用旋转矩阵。 第二种表示方法就是使用单位四元数。单位四元数可以表示为单位长度的四维向量就足够了，这里有两个需要注意的东西，一是旋转轴 u ，二是旋转角度 ϕ。 四元数不受奇点的影响，与旋转矩阵相比它只需要四个参数而不是九个。 第三种表示方法就是使用欧拉角。欧拉角之所以有吸引力，原因在于它是一种直观简约的表示，只需要知道绕每个轴转动的角度就可以得到最终的旋转变换。 将三种方法对比，旋转矩阵可以表示任何旋转，但需要九个参数并有六个约束。单位四元数也可用于表示任何旋转，但它也有一个约束。 最后，欧拉角不受约束、直观地可视化并且仅需要三个参数，但受奇点影响。

车辆定位时将使用参考坐标系。

惯性测量单元(IMU)测量物体在惯性空间中的运动。一般来说，惯性测量单元是一个复合传感器套件，它结合了三轴陀螺仪和三轴加速度计来跟踪刚体的外部自由运动。一些IMU还包含磁力计或指南针，以帮助跟踪方向。

现代陀螺仪包含一个微机电系统，该系统由一个小的硅音叉组成，该系统根据施加的旋转或方向变化来改变其共振特性。它们测量旋转速率，而不是直接测量方向，因此必须对输出信号进行数值积分以确定方向变化。这个过程会在最终的方向估计中引入额外的误差。

其测量模型为 ω ( t ) = ω s ( t ) + b g y r o ( t ) + n g y r o ( t ) \omega (t) = \omega_{s}(t)+b_{gyro}(t)+n_{gyro}(t) ω(t)=ωs​(t)+bgyro​(t)+ngyro​(t) 其中   ω s ( t ) \ \omega_{s}(t)  ωs​(t)是陀螺仪测量值， 测量模型中包含一个缓慢变化的偏置项   b g y r o ( t ) \ b_{gyro}(t)  bgyro​(t) 和一个白高斯加性噪声项   n g y r o ( t ) \ n_{gyro}(t)  ngyro​(t)来模拟传感器误差。

加速度计测量沿单个轴的加速度。当传感器加速时，光束发生偏转。可以通过电容电路测量这种偏转，并将其转换为加速度值。

加速度计测量模型将具有类似的噪声和偏置项。但是现在，我们不再像使用旋转角速度那样直接测量身体加速度，而是需要使用重力场中加速度计的基本方程来消除重力的影响。公式如下： a ( t ) = C s n ( t ) ( r ¨ n s n ( t ) − g n ) + b a c c e l ( t ) + n a c c e l ( t ) a(t) = C_{sn}(t)(\ddot{r} _{n}^{sn}(t) -g_{n})+b_{accel}(t)+n_{accel}(t) a(t)=Csn​(t)(r¨nsn​(t)−gn​)+baccel​(t)+naccel​(t)   C s n ( t ) \ C_{sn}(t)  Csn​(t)为方向矩阵

IMU模型的一些限制：

GPS系统由位于六个轨道平面的 24-32 颗卫星组成。卫星会定期退役和更换。高度约为 20,000 公里，轨道周期不到 12 小时。系统的设计使得至少四颗卫星在地球上的任何表面点始终可见。

GPS背后的基本原理是到达时间测距。接收器通过将自己的内部时钟与传输时间的时钟进行比较来计算每个可见卫星的距离。使用电磁信号以光速传播的知识将时间差转换为距离。为了计算 3D 位置，测距方程需要至少四颗可见卫星。如果已知高度并且只需要 2D 位置，则只需要三颗卫星。

对于每颗卫星，伪距测量方程为： ρ ( i ) = c ( t r − t s ) = ( p ( i ) − r ) T ( p ( i ) − r ) + c △ t r + c △ t n ( i ) + η ( i ) \rho ^{(i)} = c(t_{r}-t_{s}) = \sqrt{(p^{(i)}-r)^{T}(p^{(i)}-r) }+c\bigtriangleup t_{r}+ c\bigtriangleup t_{n}^{(i)}+\eta ^{(i)} ρ(i)=c(tr​−ts​)=(p(i)−r)T(p(i)−r) ​+c△tr​+c△tn(i)​+η(i) GPS 存在多种误差源：

我们可以通过以各种方式增强系统来提高GNSS精度。

LiDAR（激光雷达）可以感知到汽车四周所有方向并且能够提供非常准确的距离信息。如果要在一维空间中构建一个基本的 LIDAR，通常需要三个组件：一个激光器、一个光电探测器和一个非常精确的秒表。其原理是：

下图是一个原始的 LiDAR 扫描到的环境信息。中间的黑洞是传感器本身（激光雷达）所在的盲点，从那里向外扩散的同心圆是旋转的 Velodyne 传感器产生的扫描线。扫描中的每个点都根据返回信号的强度着色。 3D 扫描中的整个点集合称为点云（Point Clouds）

激光雷达通常使用球坐标 来表示单个点位置，我们使用r表示从激光雷达中心原点到 3D 点的距离，用ϵ表示从传感器 XY 平面向上或向下测量的俯仰角度，用α表示传感器 X 轴逆时针测量的方位角 将球坐标转化为笛卡尔坐标，我们可以得到3D雷达和2D雷达的传感器模型以及逆传感器模型

3D LiDAR 的逆传感器模型为： [ x y z ] = h − 1 ( r , α , ϵ ) [ r cos ⁡ α cos ⁡ ϵ r sin ⁡ α cos ⁡ ϵ r sin ⁡ ϵ ] \begin{bmatrix} x\\ y\\ z \end{bmatrix}=h^{-1} (r,\alpha ,\epsilon )\begin{bmatrix} r\cos \alpha\cos \epsilon \\ r\sin \alpha\cos \epsilon \\ r\sin \epsilon \end{bmatrix} ​xyz​ ​=h−1(r,α,ϵ) ​rcosαcosϵrsinαcosϵrsinϵ​ ​ 3D LiDAR 的前向传感器模型为： [ r α ϵ ] = h ( x , y , z ) [ x 2 + y 2 + z 2 tan ⁡ − 1 ( y x ) sin ⁡ − 1 ( z x 2 + y 2 + z 2 ) ] \begin{bmatrix} r\\ \alpha\\ \epsilon \end{bmatrix}=h (x,y,z )\begin{bmatrix} \sqrt{x^{2}+y^{2}+z^{2}}\\ \tan^{-1} (\frac{y}{x}) \\ \sin^{-1} (\frac{z}{\sqrt{x^{2}+y^{2}+z^{2}}}) \end{bmatrix} ​rαϵ​ ​=h(x,y,z) ​x2+y2+z2 ​tan−1(xy​)sin−1(x2+y2+z2 ​z​)​ ​

2D LiDAR 的逆传感器模型为： [ x y 0 ] = h − 1 ( r , α , 0 ) [ r cos ⁡ α r sin ⁡ α 0 ] \begin{bmatrix} x\\ y\\ 0 \end{bmatrix}=h^{-1} (r,\alpha ,0 )\begin{bmatrix} r\cos \alpha\\ r\sin \alpha \\ 0 \end{bmatrix} ​xy0​ ​=h−1(r,α,0) ​rcosαrsinα0​ ​ 2D LiDAR 的前向传感器模型为： [ r α 0 ] = h ( x , y , 0 ) [ x 2 + y 2 tan ⁡ − 1 ( y x ) 0 ] \begin{bmatrix} r\\ \alpha\\ 0 \end{bmatrix}=h (x,y,0 )\begin{bmatrix} \sqrt{x^{2}+y^{2}}\\ \tan^{-1} (\frac{y}{x}) \\ 0 \end{bmatrix} ​rα0​ ​=h(x,y,0) ​x2+y2 ​tan−1(xy​)0​ ​

对于激光雷达传感器，有几个重要的噪声源需要考虑。

这些因素通常通过球坐标上的附加零均值高斯噪声来解释。 含有噪声的前向传感器模型为： [ r α ϵ ] = h ( x , y , z ) [ x 2 + y 2 + z 2 tan ⁡ − 1 ( y x ) sin ⁡ − 1 ( z x 2 + y 2 + z 2 ) ] + v , v ∼ N ( 0 , R ) \begin{bmatrix} r\\ \alpha\\ \epsilon \end{bmatrix}=h (x,y,z )\begin{bmatrix} \sqrt{x^{2}+y^{2}+z^{2}}\\ \tan^{-1} (\frac{y}{x}) \\ \sin^{-1} (\frac{z}{\sqrt{x^{2}+y^{2}+z^{2}}}) \end{bmatrix}+v, v\sim N(0, R) ​rαϵ​ ​=h(x,y,z) ​x2+y2+z2 ​tan−1(xy​)sin−1(x2+y2+z2 ​z​)​ ​+v,v∼N(0,R)

我们可以使用 LiDAR 扫描的所有测量值来构建点云。使用一个矩阵来存储点云数据，如下图所示 对于状态估计，世界上的物体大多保持不动，而车辆上的参考系一直在变并且从不同的角度观察世界。因此，我们考虑参考系的空间操作。点云有三个基本的空间操作：平移，旋转和缩放。

自动驾驶汽车的平面拟合的最常见和重要的应用之一确定路面的位置并预测汽车继续行驶时的位置。 3D平面等式为： z = a + b x + c y z=a+bx+cy z=a+bx+cy 我们将会使用最小二乘法来根据雷达测量得到的x,y,x值来估计a,b,c

取平方误差函数关于参数向量 x xx 的偏导数，并将其设置为 0 以找到最小值，最终所要求的参数为： x ^ = ( A T A ) − 1 A T b \hat{x}=(A^{T} A)^{-1} A^{T}b x^=(ATA)−1ATb

点云匹配问题是指同一物体在两个不同坐标系下的点云，如何去对齐两个点云并从而确定传感器在两帧之间的运动。解决这类问题最常用的算法称为迭代最近点算法或简称 ICP。

ICP 的思想是，我们对平移和旋转设定一个初始值   { C ˇ s ′ s , r ˇ s s ′ s } \ \left \{ \check{C} _{s^{'}s } ,\check{r}_{s}^{s^{'}s} \right \}  {Cˇs′s​,rˇss′s​} 然后对   P s ′ \ {P} _{s^{'} }  Ps′​ 中的每一个点找到   P s \ {P} _{s }  Ps​中与它距离最近的点；然后求解最佳参数   { C ^ s ′ s , r ^ s s ′ s } \ \left \{ \hat{C} _{s^{'}s } ,\hat{r}_{s}^{s^{'}s} \right \}  {C^s′s​,r^ss′s​} 。以此类推，不停的迭代，直到最终收敛为止。

我们的目标是找到最能对齐两个点云的旋转和平移参数。 具体来说，我们希望使用最小二乘法最小化每对匹配点之间的距离平方之和，这是我们在此处定义的损失函数之一，公式为： { C ˇ s ′ s , r ˇ s s ′ s } = a r g m i n { C s ′ , r s s ′ } = a r g m i n ∑ j = 1 n ∥ C s ′ s ( p s ( j ) − r s s ′ s − p s ′ ( j ) ) ∥ 2 2 \left \{ \check{C} _{s^{'}s } ,\check{r}_{s}^{s^{'}s} \right \} =argmin\left \{ {C} _{s^{'}} ,{r}_{s}^{s^{'}} \right \}=argmin\sum_{j=1}^{n}\left \|{C} _{s^{'}s}({p}_{s}^{(j)}-{r}_{s}^{s^{'}s}-{p}_{s^{'}}^{(j)} ) \right \|_{2}^{2} {Cˇs′s​,rˇss′s​}=argmin{Cs′​,rss′​}=argminj=1∑n​ ​Cs′s​(ps(j)​−rss′s​−ps′(j)​) ​22​ 最优转换的方法，总共包含四步：

目前为止，我们一直假设我们的激光雷达看到的物体是静止的。但是在存在移动物体的情况下还继续使用 ICP 会使我们的运动估计远离真实运动。因此，我们需要小心排除或减轻违反我们对静止世界假设的异常点的影响。一种方法是将 ICP 运动估计与 GPS 和 INS 估计相结合。

处理此类异常值的一种简单的方法是选择一个不同的损失函数，该损失函数对异常值引起的误差损失比我们的平方误差损失更不敏感。具有此属性的损失函数称为鲁棒损失函数或鲁棒成本函数，有多种选择。下图是常见的四种损失函数：